#! /usr/bin/env python

'''
这是query_controller的实现类，该类可以随需求而重写，不过要保证继承那些接口

'''
from time import sleep
import rospy
import sys
sys.path.append("/home/galm/ms_01/src/command_receiver/scripts/query_controller")
from queryController import QueryController
from command_receiver.msg import roboCmd

class QueryControllerImpl(QueryController):

    def __init__(self):
        #等待参数服务器加载
        print("[system]:正在等待服务器加载...")
        sleep(2)
        print("[system]:服务器加载完毕")
        rospy.init_node(rospy.get_param("QCName","query_controller"))
        #获取话题名称
        topicName = rospy.get_param("QCTopic","user_cmd")
        #设置发布者publisher
        self.publisher = rospy.Publisher(topicName,roboCmd,queue_size=10)
        #设置机器人状态
        '''
        状态标识：
        on:启动
        off:关闭
        nav:给定坐标跟随移动
        man:手动控制
        fcs:射击模式
        wri:书写模式
        try:测量模式
        '''
        self.stateList = ["on","off","nav","man","fcs","wri","try"]
        self.state = "off" #当前状态是off
        self.navPos = [0,0,0] #当前的导航点
        self.carVel = [0,0] #当前车的前进速度和转向速度都是0
        self.armPos = [0,0] #当前机械臂的pitch和yaw都是0
        self.shoot = False #当前发射指令关
        self.readFlag = False #read是否进入
    
    def publish(self):
        self.publisher.publish(self.load())
        rospy.loginfo("消息已发布，状态：%s",self.state)

    def check(self,cmd):
        #用于检查输入命令是否正确
        if(cmd in self.stateList):
            return True
        return False



    def read(self):
        #这里是在nav模式下，用户输入指定的坐标点
        if(self.state == "nav"):
            x = float(input("输入x坐标："))
            y = float(input("输入y坐标："))
            self.navPos = [x,y,0]
            self.publish()
        elif(self.state == "man" or self.state == "fcs"):
            #这里是当处在手动控制和火控状态下，应该实时读取键盘状态并发布
            #首先将所有控制状态复位
            rospy.loginfo("进入手动控制模式...")
            self.reset()
            self.publish()
            self.readFlag = True #标志着由另外一个线程接手控制
            #之后根据键盘状态随时读取
            while(self.readFlag):
                pass
            self.reset()
            self.publish()
            self.readFlag = False #标志另一个线程控制结束
            rospy.loginfo("已退出手动控制模式")

            
            
        


    def query(self):
        #不断询问指令
        while True:
            cmd = input("请输入指令：")
            if(cmd == "q"):
                rospy.loginfo("Bye~")
                break
            if(not self.check(cmd)):
                #如果检查不通过
                rospy.loginfo("[warning]：输入状态不正确，请重新输入")
                continue
            #如果检查通过
            self.state = cmd
            self.reset()
            #将机器人状态改为用户状态
            if(cmd == "nav" or cmd == "man" or cmd == "fcs"):
                self.read()
            else:
                #如果不需要手动控制，就直接发布消息即可
                self.publish()

    def load(self):
        msg = roboCmd()
        #此时装载所有的信息
        msg.state = self.state
        msg.navPosX = self.navPos[0]
        msg.navPosY = self.navPos[1]
        msg.navPosZ = self.navPos[2]
        msg.armPosPit = self.armPos[0]
        msg.armPosYaw = self.armPos[1]
        msg.carVelLin = self.carVel[0]
        msg.carVelRot = self.carVel[1]
        msg.shoot = self.shoot
        return msg

    def reset(self):
        self.carVel = [0,0]
        self.armPos = [0,0]
        self.shoot = False
        self.navPos = [0,0,0]

            




